#define _USE_MATH_DEFINES
#include <cmath>

#include "PhysicsUtils.h"
#include "chipmunk/chipmunk.h"

using namespace game_engine::physics;
using namespace game_engine::utils;

FLOAT32 PhysicsUtils::CONSTANT = 10.0f;

PhysicsUtils::PhysicsUtils(void)
{
}

PhysicsUtils::~PhysicsUtils(void)
{
}

const Point PhysicsUtils::makeStepInDirection(const Point &currentPosition, 
                                       const FLOAT32 angle, 
                                       const FLOAT32 speed,
                                       const FLOAT32 scale)
{
    // Get the unit length vector for the given angle
    cpVect unitVect = cpvforangle(angle * M_PI / 180.0f);

    // Apply speed to each coordinate
    unitVect.x *= speed;
    unitVect.y *= speed;

    // Apply given scale
    unitVect = cpvmult(unitVect, scale);

    // Compute new position
    cpVect temp = cpv(currentPosition.getX(), currentPosition.getY());
    cpVect result = cpvadd(temp, unitVect);

    return Point(result.x, result.y);
}

const BOOL PhysicsUtils::collisionDetected(const Rect &rectA, 
                                            const Rect &rectB)
{
    BOOL collision = false;

    // Create first boundary box
    cpBB boxA = cpBBNew(rectA.getX(), rectA.getY(), 
        (rectA.getX() + rectA.getWidth()), (rectA.getY() + rectA.getHeight()));

    // Create second boundary box
    cpBB boxB = cpBBNew(rectB.getX(), rectB.getY(), 
        (rectB.getX() + rectB.getWidth()), (rectB.getY() + rectB.getHeight()));

    // Evaluate intersection
    cpBool res = cpBBintersects(boxA, boxB);
    if (res == cpTrue)
    {
        collision = true;
    }
    else
    {
        collision = false;
    }

    return collision;
}

const Point PhysicsUtils::makeStepTowards(const Point &currentPosition, 
                                          const Point &destination, 
                                          const FLOAT32 speed,
                                          const FLOAT32 scale)
{
    // If entity has reach destination, we return the currentPosition
    if (cpvnear(cpv(currentPosition.getX(), currentPosition.getY()), 
        cpv(destination.getX(), destination.getY()), CONSTANT))
    {
        return destination;
    }

    // Linearly interpolate between currentPosition towards destination
    cpVect temp = cpvsub(cpv(destination.getX(), destination.getY()),
        cpv(currentPosition.getX(), currentPosition.getY()));
    temp = cpvnormalize_safe(temp);

    // Apply speed to each coordinate
    temp.x *= speed;
    temp.y *= speed;

    // Apply given scale
    temp = cpvmult(temp, scale);

    // Compute new position
    cpVect current = cpv(currentPosition.getX(), currentPosition.getY());
    cpVect result = cpvadd(current, temp);

    return Point(result.x, result.y);
}

const FLOAT32 PhysicsUtils::computeAngle(const Point &p1, const Point &p2)
{
    // Linearly interpolate between p1 towards p2
    cpVect temp = cpvsub(cpv(p2.getX(), p2.getY()), cpv(p1.getX(), p1.getY()));
    temp = cpvnormalize_safe(temp);

    // Calculate angle in degrees of resulting vector
    return (cpvtoangle(temp) * 180.0f) / M_PI;
}

const FLOAT32 PhysicsUtils::distSQ(const Point &p1, const Point &p2)
{
    // Calculate square distance between p1 and p2
    cpFloat distsq = cpvdistsq(cpv(p1.getX(), p1.getY()), cpv(p2.getX(), p2.getY()));
    return static_cast<FLOAT32> (distsq);
}

const FLOAT32 PhysicsUtils::dist(const Point &p1, const Point &p2)
{
    // Calculate distance between p1 and p2
    cpFloat dist = cpvdist(cpv(p1.getX(), p1.getY()), cpv(p2.getX(), p2.getY()));
    return static_cast<FLOAT32> (dist);
}

const Point PhysicsUtils::toward(const Point &p1, const Point &p2)
{
    // Linearly interpolate from p1 towards p2
    cpVect temp = cpvsub(cpv(p2.getX(), p2.getY()), cpv(p1.getX(), p1.getY()));
    temp = cpvnormalize_safe(temp);

    return Point(temp.x, temp.y);

}

const Point PhysicsUtils::scale(const Point &p, const FLOAT32 mag)
{
    cpVect temp = cpvmult(cpv(p.getX(), p.getY()), static_cast<cpFloat> (mag));
    return Point(temp.x, temp.y);
}

const BOOL PhysicsUtils::nearTo(const Point &p1, const Point &p2, const FLOAT32 d)
{
    BOOL close = false;
    if (cpvnear(cpv(p1.getX(), p1.getY()), cpv(p2.getX(), p2.getY()), static_cast<cpFloat> (d)) > 0)
    {
        close = true;
    }
    return close;
}
